/**
 * @FilePath     : \simulations\agvWorld\Src\path.cpp
 * @Description  : 实现了路径管理的相关类，包括路径的创建、获取、比较等操作。 
 *                 包含类Path和PathBase，Path类用于表示单一路径，PathBase类用于管理多个路径。
 *                 支持从XML文件中加载路径信息并进行相应的处理。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime : 2024-12-27 14:31:40
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdlib.h>
#include <stdio.h>
#include "path.h"
#include "tools.h"
#include "tinyxml2.h"
using namespace tinyxml2;

// Static data member
NodeBase* Path::node_base_ = nullptr;

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "CPath"
Path::Path(USHORT id, USHORT start_node, USHORT end_node, float velocity_limit,
           MoveDirTag move_dir, PathType type, USHORT guide_type, float max_dead_dist)
    : id_(id), start_node_(start_node), end_node_(end_node), velocity_limit_(velocity_limit),
      velocity_limit_load_(velocity_limit), move_dir_(move_dir), type_(static_cast<USHORT>(type)),
      guide_type_(guide_type), max_dead_dist_(max_dead_dist) {
    InitParam();
}

/**
 * @brief 获取路径导航类型
 *
 * 返回路径的导航类型。
 *
 * @return USHORT 路径的导航类型
 */
USHORT Path::GuideType() const 
{
    return guide_type_;
}

void Path::InitParam() 
{
    ang_head_ = 0.0f;
    stop_dist_empty_ = 0.0f;
    stop_dist_full_ = 0.0f;
    detect_plan_ = 1;
    guide_type_str_ = STR_LASER_TYPE;
    guide_map_.clear();
    guide_map_[STR_LASER_TYPE] = LASER_GUIDANCE;
    guide_map_[STR_QR_CODE_TYPE] = QR_CODE_GUIDANCE;
    guide_map_[STR_MAG_TYPE] = MAG_GUIDANCE;
    guide_map_[STR_REFLECTOR_TYPE] = REFLECTOR_GUIDANCE;
    guide_map_[STR_NO_GUIDE_TYPE] = NO_GUIDANCE;
    bits_ = 0;
    word1_ = -1;
    word2_ = -1;
}

/**
 * @brief 将字符串类型的引导类型转换为USHORT类型的引导类型
 *
 * 根据传入的字符串类型的引导类型，查找并返回相应的USHORT类型的引导类型。
 *
 * @param type_str 字符串类型的引导类型
 * @param guide_type 输出的USHORT类型的引导类型
 *
 * @return 转换成功返回true，否则返回false
 */
bool Path::ConvertGuideType(const std::string& type, USHORT& guide_type) 
{
    auto iter = guide_map_.find(type);
    if (iter == guide_map_.end()) {
        DEBUG_ERROR_OUT("FindGuideType: guideType is unknown, force set no guide type: guideType " << type);
        guide_type = NO_GUIDANCE;
        return false;
    }

    guide_type = iter->second;

    DEBUG_OUT("FindGuideType ok: guideType is " << type << ", guideType=" << guide_type);
    return true;
}

/**
 * @brief 获取路径的起始节点
 *
 * 获取路径的起始节点，并返回该节点内容。
 *
 * @return 路径的起始节点
 */
Node Path::GetStartNode() const 
{
    Node* node = node_base_->GetNode(start_node_);
    ASSERT(node != nullptr);
    if (!node) {
        DEBUG_ERROR_OUT("error: GetStartNode node is null, start_node_=" << start_node_);
        return Node();
    }
    return *node;
}

/**
 * @brief 获取路径的终点节点
 *
 * 返回路径的终点节点。如果终点节点不存在，则返回默认构造的Node对象。
 *
 * @return Node 路径的终点节点，如果节点不存在，则返回默认构造的Node对象
 */
Node Path::GetEndNode() const 
{
    Node* node = node_base_->GetNode(end_node_);
    ASSERT(node != nullptr);
    if (!node) {
        DEBUG_ERROR_OUT("error: GetEndNode node is null, end_node_=" << end_node_);
        return Node();
    }
    return *node;
}

/**
 * @brief 获取路径的起始点
 *
 * 该函数用于获取路径的起始点。
 *
 * @return 返回路径的起始点
 */
Point Path::GetStartPoint() const 
{
    Node node = GetStartNode();
    return node.GetPntObject();
}

/**
 * @brief 获取路径的终点
 *
 * 该函数返回路径的终点坐标。
 *
 * @return 返回路径的终点坐标
 */
Point Path::GetEndPoint() const 
{
    Node node = GetEndNode();
    return node.GetPntObject();
}

/**
 * @brief 比较两个Path对象是否相等
 *
 * 判断当前对象与传入的Path对象是否相等。
 *
 * @param path 要比较的Path对象
 * @return 如果两个Path对象相等，则返回true；否则返回false
 */
bool Path::operator==(Path& path) const 
{
    return (id_ == path.id_);
}

/**
 * @brief 比较两个Path对象是否不相等
 *
 * 通过比较两个Path对象的id成员变量是否不相等来判断两个对象是否不相等。
 *
 * @param path 要比较的另一个Path对象
 * @return 如果两个Path对象的id不相等，则返回true；否则返回false
 */
bool Path::operator!=(Path& path) const 
{
    return (id_ != path.id_);
}

/**
 * @brief 获取路径的大小
 *
 * 返回路径的大小。
 *
 * @return 返回路径的大小，类型为 float。
 */
float Path::Size() const 
{
    return size_;
}

/**
 * @brief 模糊节点检查阈值
 *
 * 该函数返回模糊节点检查的阈值，用于确定路径是否满足特定条件。
 *
 * @return 返回模糊节点检查的阈值，类型为float。
 */
float Path::FuzzyNodeCheckLimit() const 
{
    return 0.06f;
}

/**
 * @brief 获取路径上指定位置的姿态
 *
 * 根据给定的相对进度值（0.0到1.0之间）获取路径上对应位置的姿态。
 *
 * @param relative_progress 相对进度值，取值范围在0.0到1.0之间
 * @param posture 用于存储获取到的姿态信息的对象
 *
 * @return 如果相对进度值在有效范围内，则返回true，并将姿态信息存储在posture中；
 *         否则返回false，posture不会被修改。
 */
bool Path::GetPosture(float relative_progress, Posture& posture) 
{
    if (relative_progress < 0 || relative_progress > Size()) return false;
    Trajectory* traj = MakeTraj();
    traj->SetRelProgress(0.0f, relative_progress); // to point
    posture = traj->PostureFun(); // + angHead
    if (traj) {
        delete traj;
        traj = nullptr;
    }
    return true;
}

/**
 * @brief 返回路径的Bit位
 *
 * 该函数返回当前路径对象的Bit位。
 *
 * @return 返回路径的Bit位
 */
uint16_t Path::Bits() const 
{
    return bits_;
}

/**
 * @brief 获取路径中的两个字段
 *
 * 该函数用于获取路径对象中的两个字段，并将它们分别赋值给传入的引用参数 word1 和 word2。
 *
 * @param word1 引用参数，用于存储第一个字段
 * @param word2 引用参数，用于存储第二个字段
 */
void Path::Words(int& word1, int& word2) const 
{
    word1 = word1_;
    word2 = word2_;
}

/**
 * @brief 获取检测规划
 *
 * 获取当前路径对象的检测规划。
 *
 * @return 返回检测规划的值
 */
int Path::GetDetectPlan() const 
{
    return detect_plan_;
}

/**
 * @brief 创建路径
 *
 * @return 返回false。
 */
bool Path::Create() 
{
    return false;
}

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "PathBase".
PathBase::PathBase() : path_count_(0), path_index_(nullptr) {}

PathBase::~PathBase() 
{
    CleanUp();
}

/**
 * @brief 清理路径数据
 *
 * 该函数负责清理PathBase对象中存储的所有路径数据，包括释放内存和重置相关指针。
 */
void PathBase::CleanUp() 
{
    for (USHORT i = 0; i < path_count_; i++) {
        if (path_index_[i].ptr_ != nullptr) {
            delete path_index_[i].ptr_;
            path_index_[i].ptr_ = nullptr;
        }
    }

    if (path_index_ != nullptr) {
        free(path_index_);
        path_index_ = nullptr;
    }
    path_count_ = 0;
}

/**
 * @brief 获取指定路径ID的路径指针
 *
 * 从路径数组中查找具有指定路径ID的路径，并返回该路径的指针。
 *
 * @param path_id 要查找的路径ID
 * @return 如果找到指定路径ID的路径，则返回该路径的指针；否则返回nullptr。
 * @exception 如果在查找过程中发生异常，将输出错误信息并返回nullptr。
 */
Path* PathBase::GetPathPointer(USHORT path_id) 
{
    std::lock_guard<std::mutex> locker(path_mutex_);
    try {
        for (USHORT i = 0; i < path_count_; i++) {
            Path* path = path_index_[i].ptr_;
            if (path->id_ == path_id) return path;
        }
    } catch (...) {
        DEBUG_ERROR_OUT("abnormal#1: program crash: GetPathPointer failed; path_count_=" << path_count_);
    }
    return nullptr;
}

/**
 * @brief 替换路径函数
 *
 * 根据给定的起始节点和结束节点，替换路径数组中相应的路径。
 *
 * @param node1 起始节点
 * @param node2 结束节点
 * @param new_path 新的路径对象指针
 *
 * @return 如果替换成功返回true，否则返回false
 */
bool PathBase::OverWritePath(USHORT node1, USHORT node2, Path *new_path)
{
    std::lock_guard<std::mutex> locker(path_mutex_);

    if (!new_path) {
        DEBUG_ERROR_OUT("OverWritePath failed;pNewPath is null");
        return false;
    }

    try {
        for (USHORT i = 0; i < path_count_; i++) {
            Path *path = path_index_[i].ptr_;
            USHORT start_node = path->start_node_;
            USHORT end_node = path->end_node_;

            if (node1 == start_node && node2 == end_node) {
                path_index_[i].ptr_ = new_path;
                delete path;
                path = NULL;
                DEBUG_OUT("OverwritePath ok;uNode1=" << (int)node1 << ",uNode2=" << (int)node2);
                return true;
            }
        }
    } catch (...) {
        DEBUG_ERROR_OUT("abnormal#3: program crash: OverwritePath failed;m_uCount=" << path_count_);
    }

    return false;
}

/**
 * @brief 获取路径指针
 *
 * 根据两个节点的ID，获取它们之间的路径指针。
 *
 * @param node1 第一个节点
 * @param node2 第二个节点
 *
 * @return 返回路径指针，如果路径不存在则返回nullptr
 */
Path* PathBase::GetPathPointer(USHORT node1, USHORT node2) 
{
    std::lock_guard<std::mutex> locker(path_mutex_);
    try {
        for (USHORT i = 0; i < path_count_; i++) {
            Path* path = path_index_[i].ptr_;
            USHORT start_node = path->start_node_;
            USHORT end_node = path->end_node_;
            if (node1 == start_node && node2 == end_node) {
                return path;
            }
        }
    } catch (...) {
        DEBUG_ERROR_OUT("abnormal#2: program crash: GetPathPointer failed; path_count_=" << path_count_);
    }
    return nullptr;
}

//
//   Get the specified path object (Form #2.A).
//   FIXME: need change
// CPath* CPathBase::GetPathPointer(USHORT uNode1, USHORT uNode2, CMoveDir MoveDir)
//{
//    for (USHORT i = 0; i < m_uCount; i++)
//    {
//        CPath* pPath = m_pPathIdx[i].m_ptr;

//        USHORT uStartNode = pPath->m_uStartNode;
//        USHORT uEndNode   = pPath->m_uEndNode;

//        if ((MoveDir == FORWARD && uNode1 == uStartNode && uNode2 == uEndNode) ||
//                (MoveDir == BACKWARD && uNode1 == uEndNode && uNode2 == uStartNode))

//            return pPath;
//    }

//    return NULL;
//}


/**
 * @brief 获取路径指针
 *
 * 从路径数组中查找并返回指定起点和终点的路径指针。
 *
 * @param node1 路径的起点节点
 * @param node2 路径的终点节点
 *
 * @return 如果找到匹配路径，则返回路径指针；否则返回nullptr。
 */
Path* PathBase::GetPathPointer(Node& node1, Node& node2) 
{
    return GetPathPointer(node1.id_, node2.id_);
}

/**
 * @brief 获取路径指针
 *
 * 根据节点编号和起始节点的朝向，获取路径指针。
 *
 * @param node1 起始节点编号
 * @param node2 目标节点编号
 * @param heading_at_node1 起始节点的朝向（以度为单位）
 *
 * @return 如果找到符合条件的路径，返回路径指针；否则返回 nullptr
 */
Path* PathBase::GetPathPointer(USHORT node1, USHORT node2, float heading_at_node1) 
{
    try {
        for (USHORT i = 0; i < path_count_; i++) {
            Path* path = path_index_[i].ptr_;
            USHORT start_node = path->start_node_;
            USHORT end_node = path->end_node_;
            if (node1 == start_node && node2 == end_node) {
                Node node = path->GetStartNode();
                if (path->GetHeading(node) == Angle(heading_at_node1)) return path;
            }
        }
    } catch (...) {
        DEBUG_ERROR_OUT("abnormal#3: program crash: GetPathPointer failed; path_count_=" << path_count_);
        return nullptr;
    }
    return GetPathPointer(node1, node2);
}

/**
 * @brief 判断两个节点之间是否存在路径
 *
 * 判断两个节点（node1 和 node2）之间是否存在路径。
 *
 * @param node1 第一个节点的标识符
 * @param node2 第二个节点的标识符
 * @return 如果存在路径，则返回 true；否则返回 false
 */
bool PathBase::IsPath(USHORT node1, USHORT node2) 
{
    return (GetPathPointer(node1, node2) != nullptr);
}

/**
 * @brief 获取指定节点的邻接节点
 *
 * 在路径数组中查找给定节点的邻接节点。
 * 如果找到，则返回该邻接节点的标识；否则，返回 NULLID。
 *
 * @param node 需要查找邻接节点的节点标识
 * @return 返回找到的邻接节点标识，如果未找到，则返回 NULLID
 *
 * @note 该函数在多线程环境下是线程安全的，使用了互斥锁保护路径数组。
 * @note 如果在查找过程中出现异常，会输出错误信息并记录。
 */
USHORT PathBase::GetNeighborNode(USHORT node) 
{
    std::lock_guard<std::mutex> locker(path_mutex_);
    try {
        for (USHORT i = 0; i < path_count_; i++) {
            Path* path = path_index_[i].ptr_;
            USHORT start_node = path->start_node_;
            USHORT end_node = path->end_node_;
            if (node == start_node) return end_node;
        }
    } catch (...) {
        DEBUG_ERROR_OUT("abnormal#4: program crash: GetNeighborNode failed; path_count_=" << path_count_ << ", node=" << node);
    }
    return NULLID;
}

/**
 * @brief 向路径列表中添加一个新的路径
 *
 * 将一个新的路径添加到路径列表中，并更新路径索引。
 *
 * @param path 待添加的路径指针
 *
 * @return 如果添加成功返回true，否则返回false
 */
bool PathBase::AddPath(Path* path) 
{
    PathIndex* temp = static_cast<PathIndex*>(calloc(path_count_ + 1, sizeof(PathIndex)));
    if (temp == nullptr) return false;
    for (USHORT i = 0; i < path_count_; i++) temp[i] = path_index_[i];
    temp[path_count_++].ptr_ = path;
    if (path_index_ != nullptr) {
        free(path_index_);
        path_index_ = nullptr;
    }
    path_index_ = temp;
    return true;
}

/**
 * @brief 从路径列表中移除指定路径
 *
 * 从当前路径列表中移除指定索引的路径，并释放该路径所占用的资源。
 *
 * @param id 要移除的路径的索引
 * @return 移除成功返回 true，否则返回 false
 */
bool PathBase::RemovePath(USHORT id) 
{
    USHORT i;
    if (id >= path_count_) return true;
    PathIndex* temp = static_cast<PathIndex*>(calloc(path_count_ - 1, sizeof(PathIndex)));
    if (temp == nullptr) return false;
    for (i = 0; i < path_count_; i++) {
        if (i != id) temp[i] = path_index_[i];
        else {
            if (path_index_[id].ptr_) {
                delete path_index_[id].ptr_;
                path_index_[id].ptr_ = nullptr;
            }
            break;
        }
    }
    path_count_--;
    for (; i < path_count_; i++) temp[i] = path_index_[i + 1];
    if (path_index_ != nullptr) {
        free(path_index_);
        path_index_ = nullptr;
    }
    path_index_ = temp;
    return true;
}

/**
 * @brief 获取路径姿态
 *
 * 根据给定的起点和终点，以及相对进度，获取路径的姿态信息。
 *
 * @param from_node 起点节点ID
 * @param to_node 终点节点ID
 * @param relative_progress 相对进度，范围在[0, 1]之间，表示路径上的相对位置
 * @param posture 输出的姿态信息
 *
 * @return 如果获取成功返回true，否则返回false
 */
bool PathBase::GetPosture(USHORT from_node, USHORT to_node, float relative_progress, Posture& posture) 
{
    if (relative_progress < 0) return false;
    if ((fabs(relative_progress) < 1.0E-5) && (from_node == to_node)) {
        to_node = GetNeighborNode(from_node);
        if (to_node == NULLID) return false;
        Path* path = GetPathPointer(from_node, to_node);
        if (path == nullptr) return false;
        Node node = path->GetStartNode();
        posture = Posture(node.x_, node.y_, node.heading_);
        return true;
    }
    Path* path = GetPathPointer(from_node, to_node);
    if (path == nullptr) return false;
    if (relative_progress > path->Size()) relative_progress = path->Size();
    return path->GetPosture(relative_progress, posture);
}

/**
 * @brief 创建路径对象
 *
 * 根据给定的地图路径文件，解析XML文件，并创建相应的路径对象。
 *
 * @param map_path 地图路径文件的路径
 *
 * @return 成功创建路径对象返回true，否则返回false
 */
bool PathBase::Create(const std::string& map_path) 
{
    std::lock_guard<std::mutex> locker(path_mutex_);
    CleanUp();
    XMLDocument doc;
    if (doc.LoadFile(map_path.c_str()) != 0) {
        DEBUG_OUT("load xml file failed");
        return false;
    }

    XMLElement* root = doc.RootElement();
    if (!root) {
        DEBUG_OUT("xml root is nullptr");
        return false;
    }

    std::string map_id = xml_tool::FindMapId(root);
    XMLElement* second_root = xml_tool::FindMapData(root, map_id);
    if (!second_root) {
        DEBUG_OUT("xml secondRoot is nullptr");
        return false;
    }

    std::string path_element_name = "path";
    XMLElement* first_path_node = second_root->FirstChildElement(path_element_name.c_str());
    std::vector<XMLElement*> path_element_vector;
    while (first_path_node != nullptr) {
        std::string cur_node_name = std::string(first_path_node->Name());
        if (path_element_name == cur_node_name) {
            path_element_vector.push_back(first_path_node);
        }
        first_path_node = first_path_node->NextSiblingElement();
    }
    
    path_count_ = path_element_vector.size();
    path_index_ = static_cast<PathIndex*>(calloc(path_count_, sizeof(PathIndex)));
    if (path_index_ == nullptr) {
        return false;
    }
    for (USHORT i = 0; i < path_count_; i++) {
        Path* path;
        USHORT type;
        type = static_cast<USHORT>(PathType::LINE_TYPE);
        XMLElement* cur_element = path_element_vector.at(i);
        if (!cur_element) {
            DEBUG_OUT("path is null: i=" << i);
            return false;
        }
        auto p_temp = cur_element->FindAttribute("type");
        std::string path_type = std::string(p_temp ? p_temp->Value() : "");
        p_temp = cur_element->FindAttribute("sideMove");
        bool bSide = p_temp ? p_temp->IntValue() : 0;
        if (path_type == "LinePath" && !bSide) {
            type = static_cast<USHORT>(PathType::LINE_TYPE);
        } else if (path_type == "BezierPath") {
            type = static_cast<USHORT>(PathType::BEZIER_TYPE);
        } else if (path_type == "LinePath" && bSide) {
            type = static_cast<USHORT>(PathType::SIDE_TYPE);
        } else {
            DEBUG_OUT("unknown path type");
            return false;
        }
        switch (static_cast<PathType>(type)) {
            case PathType::LINE_TYPE: {
                path = new LinePath;
                p_temp = cur_element->FindAttribute("id");
                path->id_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("startPointId");
                path->start_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("endPointId");
                path->end_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("lineVelocity");
                path->velocity_limit_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("loadLineVelocity");
                path->velocity_limit_load_ = p_temp ? p_temp->FloatValue() : path->velocity_limit_;
                p_temp = cur_element->FindAttribute("guideType");
                path->guide_type_str_ = p_temp ? p_temp->Value() : STR_LASER_TYPE;
                path->ConvertGuideType(path->guide_type_str_, path->guide_type_);
                path->max_dead_dist_ = MAX_DEAD_DIST;
                p_temp = cur_element->FindAttribute("direction");
                std::string dir = p_temp ? p_temp->Value() : "";
                path->move_dir_ = dir == "forward" ? MoveDirTag::Forward : MoveDirTag::Backward;
                p_temp = cur_element->FindAttribute("stopDisEmpty");
                path->stop_dist_empty_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("stopDistFull");
                path->stop_dist_full_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("avoidType");
                path->detect_plan_ = p_temp ? p_temp->IntValue() : 1;
                p_temp = cur_element->FindAttribute("bits");
                path->bits_ = p_temp ? p_temp->IntValue() : 0;
                p_temp = cur_element->FindAttribute("word1");
                path->word1_ = p_temp ? p_temp->IntValue() : -1;
                p_temp = cur_element->FindAttribute("word2");
                path->word2_ = p_temp ? p_temp->IntValue() : -1;
                DEBUG_OUT("LinePath::Create: i=" << i
                          << ", id_=" << path->id_
                          << ", start_node_=" << path->start_node_
                          << ", end_node_=" << path->end_node_
                          << ", velocity_limit_=" << path->velocity_limit_
                          << ", velocity_limit_load_=" << path->velocity_limit_load_
                          << ", guide_type_=" << path->guide_type_
                          << ", max_dead_dist_=" << path->max_dead_dist_
                          << ", move_dir_=" << static_cast<int>(path->move_dir_)
                          << ", detect_plan_=" << path->detect_plan_
                          << ", guide_type_str_=" << path->guide_type_str_
                          << ", bits_=" << path->bits_
                          << ", word1_=" << path->word1_
                          << ", word2_=" << path->word2_);
            }
            break;
            case PathType::BEZIER_TYPE: {
                BezierPath* bezier_path = new BezierPath;
                path = bezier_path;
                p_temp = cur_element->FindAttribute("id");
                bezier_path->id_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("startPointId");
                bezier_path->start_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("endPointId");
                bezier_path->end_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("lineVelocity");
                bezier_path->velocity_limit_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("loadLineVelocity");
                bezier_path->velocity_limit_load_ = p_temp ? p_temp->FloatValue() : bezier_path->velocity_limit_;
                p_temp = cur_element->FindAttribute("guideType");
                path->guide_type_str_ = p_temp ? p_temp->Value() : STR_LASER_TYPE;
                path->ConvertGuideType(path->guide_type_str_, path->guide_type_);
                bezier_path->max_dead_dist_ = MAX_DEAD_DIST;
                p_temp = cur_element->FindAttribute("direction");
                std::string dir = p_temp ? p_temp->Value() : "";
                bezier_path->move_dir_ = dir == "forward" ? MoveDirTag::Forward : MoveDirTag::Backward;
                p_temp = cur_element->FindAttribute("stopDisEmpty");
                bezier_path->stop_dist_empty_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("stopDistFull");
                bezier_path->stop_dist_full_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("avoidType");
                bezier_path->detect_plan_ = p_temp ? p_temp->IntValue() : 1;
                p_temp = cur_element->FindAttribute("control1PosX");
                bezier_path->ctrl_points_[0].x_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("control1PosY");
                bezier_path->ctrl_points_[0].y_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("control2PosX");
                bezier_path->ctrl_points_[1].x_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("control2PosY");
                bezier_path->ctrl_points_[1].y_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("sideMove");
                bezier_path->is_shift_ = p_temp ? p_temp->IntValue() : 0;
                p_temp = cur_element->FindAttribute("angHead");
                bezier_path->ang_head_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("bits");
                bezier_path->bits_ = p_temp ? p_temp->IntValue() : 0;
                p_temp = cur_element->FindAttribute("word1");
                bezier_path->word1_ = p_temp ? p_temp->IntValue() : -1;
                p_temp = cur_element->FindAttribute("word2");
                bezier_path->word2_ = p_temp ? p_temp->IntValue() : -1;
                DEBUG_OUT("BezierPath: i=" << i
                          << ", id_=" << bezier_path->id_
                          << ", start_node_=" << bezier_path->start_node_
                          << ", end_node_=" << bezier_path->end_node_
                          << ", velocity_limit_=" << bezier_path->velocity_limit_
                          << ", velocity_limit_load_=" << bezier_path->velocity_limit_load_
                          << ", guide_type_=" << bezier_path->guide_type_
                          << ", max_dead_dist_=" << bezier_path->max_dead_dist_
                          << ", ctrl_point1.x=" << bezier_path->ctrl_points_[0].x_
                          << ", ctrl_point1.y=" << bezier_path->ctrl_points_[0].y_
                          << ", ctrl_point2.x=" << bezier_path->ctrl_points_[1].x_
                          << ", ctrl_point2.y=" << bezier_path->ctrl_points_[1].y_
                          << ", detect_plan_=" << bezier_path->detect_plan_
                          << ", guide_type_str_=" << bezier_path->guide_type_str_
                          << ", is_shift_=" << bezier_path->is_shift_
                          << ", ang_head_=" << bezier_path->ang_head_
                          << ", bits_=" << bezier_path->bits_
                          << ", word1_=" << bezier_path->word1_
                          << ", word2_=" << bezier_path->word2_);
            }
            break;
            case PathType::SIDE_TYPE: {
                path = new SidePath;
                p_temp = cur_element->FindAttribute("id");
                path->id_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("startPointId");
                path->start_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("endPointId");
                path->end_node_ = p_temp ? p_temp->IntValue() : 65535;
                p_temp = cur_element->FindAttribute("lineVelocity");
                path->velocity_limit_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("loadLineVelocity");
                path->velocity_limit_load_ = p_temp ? p_temp->FloatValue() : path->velocity_limit_;
                p_temp = cur_element->FindAttribute("guideType");
                path->guide_type_str_ = p_temp ? p_temp->Value() : STR_LASER_TYPE;
                path->ConvertGuideType(path->guide_type_str_, path->guide_type_);
                path->max_dead_dist_ = MAX_DEAD_DIST;
                p_temp = cur_element->FindAttribute("stopDisEmpty");
                path->stop_dist_empty_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("stopDistFull");
                path->stop_dist_full_ = p_temp ? p_temp->FloatValue() : 0.0f;
                p_temp = cur_element->FindAttribute("avoidType");
                path->detect_plan_ = p_temp ? p_temp->IntValue() : 1;
                p_temp = cur_element->FindAttribute("bits");
                path->bits_ = p_temp ? p_temp->IntValue() : 0;
                p_temp = cur_element->FindAttribute("word1");
                path->word1_ = p_temp ? p_temp->IntValue() : -1;
                p_temp = cur_element->FindAttribute("word2");
                path->word2_ = p_temp ? p_temp->IntValue() : -1;
                p_temp = cur_element->FindAttribute("angHead");
                path->ang_head_ = p_temp ? p_temp->FloatValue() : 0.0f;
                DEBUG_OUT("SidePath::Create: i=" << i
                          << ", id_=" << path->id_
                          << ", start_node_=" << path->start_node_
                          << ", end_node_=" << path->end_node_
                          << ", velocity_limit_=" << path->velocity_limit_
                          << ", velocity_limit_load_=" << path->velocity_limit_load_
                          << ", guide_type_=" << path->guide_type_
                          << ", max_dead_dist_=" << path->max_dead_dist_
                          << ", detect_plan_=" << path->detect_plan_
                          << ", guide_type_str_=" << path->guide_type_str_
                          << ", ang_head_=" << path->ang_head_
                          << ", bits_=" << path->bits_
                          << ", word1_=" << path->word1_
                          << ", word2_=" << path->word2_);
            }
            break;
            default:
                DEBUG_OUT("unknown path type path");
                return false;
        }

        if (path != nullptr && path->Create()) {
            path->type_ = type;
            path_index_[i].ptr_ = path;
        } else {
            DEBUG_OUT("create path is error (nullptr)");
            return false;
        }

        DEBUG_OUT("Path: type=" << type);
    }
    return true;
}
